﻿#ifndef KINEMATICS_H
#define KINEMATICS_H

#include <vector>
#include <iostream>
#include <QVector>
#include "mathtype.h"
#include <Eigen/Eigen>


#define PI 3.141592653589793
#define RAD2DEG 57.295779513082323
#define DEG2RAD 0.017453292519943
namespace CIRT_LBR {

class Transform
{
public:
    Transform();
    Transform(double _nx,double _ny,double _nz,
              double _ox,double _oy,double _oz,
              double _ax,double _ay,double _az,
              double _px,double _py,double _pz);
    Transform(const Transform& T);
    Transform(const Transformation* T);
    Transform(const Eigen::Matrix4d& T);
    Transform(const QVector<double> pos);
    Transform& operator=(const Transform& T);
    Transform& operator-=(const Transform& T);
    double abs();
    Transform operator*(const Transform& T);
    Transformation* totransformation();
    Transform inversed();
    QVector<double> toVector();
    friend std::ostream& operator<<(std::ostream& out, const Transform& T);
    void toeulerangle();
    double nx,ny,nz,ox,oy,oz,ax,ay,az,px,py,pz;
};

Transform LBR_fk(const QVector<double> &Q);
void LBR_fk(const VTYPE Q[6], Transformation* T);

QVector<QVector<double> > LBR_ik(const Transform& T);
void LBR_ik(const Transformation* T, unsigned int* solutionnum, VTYPE inversesolutions[8][6]);

std::vector<std::pair<double,double> > phase_shift_eq(const double& a,const double& b, const double& c);
unsigned char phase_shift_eq(VTYPE sin_cos_pairs[2][2], const VTYPE a, const VTYPE b, const VTYPE c);

bool isequal(const QVector<double> &a, const QVector<double> &b);

void sincosround(double& value);
void sincosround(VTYPE* value);

double maxdistance(const QVector<double> &joint1, const QVector<double> &joint2);
QVector<float> nearstsloution(const QVector<QVector<double> > sluts, const QVector<double> current, double tolrance = 180);

VTYPE caldistance(const VTYPE Q1[6], const VTYPE Q2[6]);

}
#endif // KINEMATICS_H
